#include "mpc.h"
#include <iostream>
#include <yaml-cpp/yaml.h>
#include <chrono>
#include <string>
#include "vehicle_lmpc_base.hpp"

namespace auto_ros
{
namespace control
{
class VehicleLateralSpeedYawMpc : public VehicleLMpcBase
{
public:
	VehicleLateralSpeedYawMpc(const std::string dyn_mpc_kf_config_path_name, const std::string vehicle_lmpc_yaml_name, double rate);
	virtual void init_mpc();
	virtual bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					  const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					  double speed, const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, double tire_f_alpha_w,
					  double tire_r_alpha_w);
	virtual bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, double kappa, double speed); //q r weighting factor will regulate auto based on yaml file
	virtual bool reset_kf();
	virtual bool reset_kf(const Eigen::VectorXd &state_esti);
};

} // namespace control
} // namespace auto_ros